function [P] = CalculateCameraProjectionMatrix( Xp,xp)
global X;
global x;

n=size(Xp,2);

if (n~=size(xp,2))
    error('length of x and X must equal');
end

%prepare the X and x matrixes
X=zeros(4, n);
size(Xp)
size(X)
X(1:3,:) =Xp;
X(4,:)=1;


x=zeros(3, n);
x(1:2,:) =xp;
x(3,:)=1;




std_x=std(x');
center_x=sum(x')./n;
center_x=center_x./std_x;
T=[1/std_x(1) 0 -center_x(1);
    0 1/std_x(2) -center_x(2);
    0 0 1];



x=T*x;

std_X=std(X');
center_X=sum(X')./n;
center_X=center_X./std_X;
U=[1/std_X(1) 0 0 -center_X(1);
    0 1/std_X(2) 0 -center_X(2);
    0 0 1/std_X(3) -center_X(3);
    0 0 0 1];
X=U*X;

%%%%%%%%%%%%%
    
%%%% DLT %%%%

%create matrix A
A=zeros(3*n,12);

for i=1:n
    
    %A(2*i-1,1:4)=0;
    A(3*i-2,5:8)=-x(3,i).*(X(:,i))';
    A(3*i-2,9:12)=x(2,i).*(X(:,i))';
    
    A(3*i-1,1:4)=x(3,i).*(X(:,i))';
    %A(2*i,5:8)=0;
    A(3*i-1,9:12)=-x(1,i).*(X(:,i))';
    
    A(3*i,1:4)=-x(2,i).*(X(:,i))';
    A(3*i,5:8)=x(1,i).*(X(:,i))';
end

[W,S,V] = svd(A);
init_guess=V(:,end);

p=lsqnonlin(@geom_dist,init_guess);
%p = init_guess; 
P(1,:)=p(1:4);
P(2,:)=p(5:8);
P(3,:)=p(9:12);

%%%%%%%
%Un-Normalize P
P=inv(T)*P*U;
end


function F=geom_dist(p)
global X;
global x;

P(1,:)=p(1:4);
P(2,:)=p(5:8);
P(3,:)=p(9:12);

comp_x=P*X;
for i=1:3
    comp_x(i,:) = comp_x(i,:)./comp_x(3,:);
end
dif=x-comp_x;

F=(sqrt((dif(1,:).^2)+(dif(2,:).^2)+(dif(3,:).^2)))';
end